#include "foc.h"
#include "arm_math.h"
#include "motor_runtime_param.h"
#include <stdbool.h>
#define deg2rad(a) (PI * (a) / 180)
#define rad2deg(a) (180 * (a) / PI)
#define max(a, b) ((a) > (b) ? (a) : (b))
#define min(a, b) ((a) < (b) ? (a) : (b))
#define rad60 deg2rad(60)
#define SQRT3 1.73205080756887729353

arm_pid_instance_f32 pid_position;
arm_pid_instance_f32 pid_speed;
arm_pid_instance_f32 pid_torque_d;
arm_pid_instance_f32 pid_torque_q;
motor_control_context_t motor_control_context;


//这段代码实现了 空间矢量脉宽调制（SVPWM, Space Vector PWM） 算法，用于三相逆变器（如电机驱动）的电压控制。以下是逐步解析：
//
//1. 函数功能
//输入：
//phi：当前电角度（弧度）。
//d, q：转子坐标系下的电压分量（标幺值，范围[-1, 1]）。
//输出：
//*d_u, *d_v, *d_w：三相PWM的占空比（范围[0, 1]）。
//核心操作：
//将
//d
///
//q
//d/q 电压反Park变换到静止坐标系（
//α
///
//β
//α/β）。
//
//确定扇区（Sector）并计算基本矢量作用时间。
//
//合成三相占空比。
static void svpwm(float phi, float d, float q, float *d_u, float *d_v, float *d_w)
{
    //确保  d /qd/q 分量在 [-1, 1] 范围内，防止过调制
    d = min(d, 1);
    d = max(d, -1);
    q = min(q, 1);
    q = max(q, -1);
    //矢量表
    const int v[6][3] = {{1, 0, 0}, {1, 1, 0}, {0, 1, 0}, {0, 1, 1}, {0, 0, 1}, {1, 0, 1}};
    const int K_to_sector[] = {4, 6, 5, 5, 3, 1, 2, 2};
    float sin_phi = arm_sin_f32(phi);
    float cos_phi = arm_cos_f32(phi);
    float alpha = 0;
    float beta = 0;
    //反变换 获取aipha  beta的值
    arm_inv_park_f32(d, q, &alpha, &beta, sin_phi, cos_phi);


    bool A = beta > 0;
    bool B = fabs(beta) > SQRT3 * fabs(alpha);
    bool C = alpha > 0;

    int K = 4 * A + 2 * B + C;
    int sector = K_to_sector[K];

    float t_m = arm_sin_f32(sector * rad60) * alpha - arm_cos_f32(sector * rad60) * beta;
    float t_n = beta * arm_cos_f32(sector * rad60 - rad60) - alpha * arm_sin_f32(sector * rad60 - rad60);
    float t_0 = 1 - t_m - t_n;

    *d_u = t_m * v[sector - 1][0] + t_n * v[(sector) % 6][0] + t_0 / 2;
    *d_v = t_m * v[sector - 1][1] + t_n * v[(sector) % 6][1] + t_0 / 2;
    *d_w = t_m * v[sector - 1][2] + t_n * v[(sector) % 6][2] + t_0 / 2;
}

__attribute__((weak)) void set_pwm_duty(float d_u, float d_v, float d_w)
{
    while (1)
        ;
}

void foc_forward(float d, float q, float rotor_rad)
{
    float d_u = 0;
    float d_v = 0;
    float d_w = 0;
    svpwm(rotor_rad, d, q, &d_u, &d_v, &d_w);
    set_pwm_duty(d_u, d_v, d_w);
}
//这个 position_loop 函数实现了一个 位置闭环控制，通常用于电机或机械系统的位置控制（如舵机、机械臂关节）。以下是详细解析
static float position_loop(float rad)
{
    float diff = cycle_diff(rad - motor_logic_angle, position_cycle);
    return arm_pid_f32(&pid_position, diff);
}

static float speed_loop(float speed_rad)
{
    float diff = speed_rad - motor_speed;
    return arm_pid_f32(&pid_speed, diff);
}

// torque_d_loop 是一个静态函数，返回类型为 float，它接受一个 float 类型的参数 d。
// 此函数的主要功能是实现一个带有积分饱和处理的 PID（比例 - 积分 - 微分）控制器，用于计算电机的转矩控制信号。
//static float torque_d_loop(float d)
//{
//  //// 计算当前输入值 d 与电机电流 i_d 经过归一化处理后的值之间的差值
//    float diff = d - motor_i_d / MAX_CURRENT;
//    float out_unlimited = arm_pid_f32(&pid_torque_d, diff);
//    float out_limited = 0;
//    out_limited = min(out_unlimited, 1);
//    out_limited = max(out_limited, -1);
//
//    float error_integral_windup = out_limited - out_unlimited; //  积分饱和误差
//    float Kt = 0.7f;                                           //  后积分增益 (Anti-windup Gain)
//    pid_torque_d.state[0] -= (pid_torque_d.Ki * Kt * error_integral_windup);
//
//    return out_limited;
//}

static float torque_d_loop(float d)
{
    // 计算当前输入值 d 与电机电流 i_d 经过归一化处理后的值之间的差值
    float diff = d - motor_i_d / MAX_CURRENT;

    // 调用 arm_pid_f32 函数，将差值作为输入传递给 PID 控制器
    // 该函数会根据 PID 控制器的参数计算输出值，结果存储在 out_unlimited 中
    float out_unlimited = arm_pid_f32(&pid_torque_d, diff);

    // 初始化一个变量 out_limited 用于存储经过限幅处理后的输出值
    float out_limited = 0;

    // 将 out_unlimited 的值限制在 -1 到 1 的范围内
    // 首先将 out_unlimited 和 1 进行比较，取较小值，确保输出不超过 1
    out_limited = min(out_unlimited, 1);
    // 然后将 out_limited 和 -1 进行比较，取较大值，确保输出不低于 -1
    out_limited = max(out_limited, -1);

    // 计算积分饱和误差，即限幅后的输出值与未限幅的输出值之间的差值
    float error_integral_windup = out_limited - out_unlimited;

    // 定义后积分增益 Kt，用于处理积分饱和问题
    float Kt = 0.7f;

    // 根据积分饱和误差和后积分增益，调整 PID 控制器的积分项
    // 这样可以防止积分项过度累积，避免出现积分饱和现象
    pid_torque_d.state[0] -= (pid_torque_d.Ki * Kt * error_integral_windup);

    // 返回经过限幅处理后的输出值
    return out_limited;
}

static float torque_q_loop(float q)
{
    float diff = q - motor_i_q / MAX_CURRENT;
    float out_unlimited = arm_pid_f32(&pid_torque_q, diff);
    float out_limited = 0;
    out_limited = min(out_unlimited, 1);
    out_limited = max(out_limited, -1);

    float Kt = 0.7f;                                           //  后积分增益 (Anti-windup Gain)
    float error_integral_windup = out_limited - out_unlimited; //  积分饱和误差
    pid_torque_q.state[0] -= (pid_torque_q.Ki * Kt * error_integral_windup);

    return out_unlimited;
}

//void lib_position_control(float rad)
//{
//    float d = 0;
//    float q = position_loop(rad);
//    foc_forward(d, q, rotor_logic_angle);
//}
void lib_position_control(float rad)
{
    // 初始化变量 d 为 0
    float d = 0;

    // 调用 position_loop 函数，将输入的 rad 作为参数传入
    // position_loop 函数可能实现了某种位置控制算法，根据输入的 rad 计算出一个值并返回
    // 将返回值赋给变量 q
    float q = position_loop(rad);

    // 调用 foc_forward 函数，将 d、q 和 rotor_logic_angle 作为参数传入
    // foc_forward 函数可能实现了磁场定向控制（Field Oriented Control，FOC）的前向转换算法
    // 该函数会根据输入的 d、q 分量和转子逻辑角度 rotor_logic_angle 进行相应的计算和控制操作
    foc_forward(d, q, rotor_logic_angle);
}

//void lib_speed_control(float speed)
//{
//    float d = 0;
//    float q = speed_loop(speed);
//    foc_forward(d, q, rotor_logic_angle);
//}
void lib_speed_control(float speed)
{
    // 初始化变量 d 为 0。在磁场定向控制（FOC）中，d 轴通常与磁链相关，这里初始化为 0 可能意味着在该控制场景下，不考虑 d 轴的额外控制量。
    float d = 0;

    // 调用 speed_loop 函数，将传入的 speed 作为参数。
    // speed_loop 函数大概率实现了速度控制算法，像 PID 控制算法就可能包含在内。
    // 它会依据输入的目标速度 speed 与当前实际速度的误差，计算得出一个控制量 q。
    // 该控制量 q 用于后续的 FOC 控制，一般和电机的转矩相关。
    float q = speed_loop(speed);

    // 调用 foc_forward 函数，将 d、q 和 rotor_logic_angle 作为参数传入。
    // foc_forward 函数实现的是磁场定向控制（FOC）的前向转换算法。
    // 此算法会把 d、q 坐标系下的控制量转换为三相坐标系下的控制量，从而驱动电机。
    // rotor_logic_angle 表示转子的逻辑角度，是 FOC 控制中必不可少的参数。
    foc_forward(d, q, rotor_logic_angle);
}

//void lib_torque_control(float torque_norm_d, float torque_norm_q)
//{
//    float d = torque_d_loop(torque_norm_d);
//    float q = torque_q_loop(torque_norm_q);
//    foc_forward(d, q, rotor_logic_angle);
//}
void lib_torque_control(float torque_norm_d, float torque_norm_q)
{
    // 调用 torque_d_loop 函数，将归一化的 d 轴转矩指令 torque_norm_d 作为输入
    // torque_d_loop 函数可能实现了一个针对 d 轴的闭环控制算法（如 PID 控制）
    // 该函数会根据输入的转矩指令计算出合适的 d 轴控制量，并将结果存储在变量 d 中
    float d = torque_d_loop(torque_norm_d);

    // 调用 torque_q_loop 函数，将归一化的 q 轴转矩指令 torque_norm_q 作为输入
    // torque_q_loop 函数可能实现了一个针对 q 轴的闭环控制算法（如 PID 控制）
    // 该函数会根据输入的转矩指令计算出合适的 q 轴控制量，并将结果存储在变量 q 中
    float q = torque_q_loop(torque_norm_q);

    // 调用 foc_forward 函数，将计算得到的 d 轴和 q 轴控制量 d 和 q 以及转子逻辑角度 rotor_logic_angle 作为输入
    // foc_forward 函数实现了磁场定向控制（FOC）的前向变换算法
    // 它会将 dq 坐标系下的控制量转换为三相坐标系下的控制量，用于驱动电机，从而实现对电机转矩的控制
    foc_forward(d, q, rotor_logic_angle);
}


//lib_speed_torque_control 函数的主要目的是结合速度控制和转矩控制，实现对电机运行的更精确调控。
//    它接收两个参数：目标速度 speed_rad 和最大允许的归一化转矩 max_torque_norm。
//    函数会先根据目标速度计算出所需的转矩指令，然后对该转矩指令进行限幅处理，最后调用 lib_torque_control 函数来执行转矩控制。
//void lib_speed_torque_control(float speed_rad, float max_torque_norm)
//{
//    float torque_norm = speed_loop(speed_rad);
//    torque_norm = min(fabs(torque_norm), max_torque_norm) * (torque_norm > 0 ? 1 : -1);
//    lib_torque_control(0, torque_norm);
//}
void lib_speed_torque_control(float speed_rad, float max_torque_norm)
{
    // 调用 speed_loop 函数，将目标速度 speed_rad 作为输入
    // speed_loop 函数通常实现了速度控制算法，例如 PID 控制器
    // 该函数会根据目标速度和当前实际速度的误差，计算出所需的转矩指令
    // 计算结果存储在变量 torque_norm 中
    float torque_norm = speed_loop(speed_rad);

    // 对计算得到的转矩指令进行限幅处理
    // fabs(torque_norm) 计算转矩指令的绝对值
    // min(fabs(torque_norm), max_torque_norm) 取转矩指令绝对值和最大允许转矩之间的较小值
    // (torque_norm > 0 ? 1 : -1) 用于保留转矩指令的符号（正或负）
    // 最终将限幅后的转矩指令重新赋值给 torque_norm
    torque_norm = min(fabs(torque_norm), max_torque_norm) * (torque_norm > 0 ? 1 : -1);

    // 调用 lib_torque_control 函数，将 d 轴转矩指令设为 0，q 轴转矩指令设为限幅后的 torque_norm
    // lib_torque_control 函数会根据输入的 d 轴和 q 轴转矩指令，执行转矩控制操作
    lib_torque_control(0, torque_norm);
}
//lib_position_speed_torque_control 函数实现了一种综合的控制策略，结合了位置控制、速度控制和转矩控制。
//        该函数接收三个参数：目标位置 position_rad、最大允许速度 max_speed_rad 和最大允许归一化转矩 max_torque_norm。
//            其主要作用是先根据目标位置计算出所需的速度指令，对速度指令进行限幅处理，
//        然后将限幅后的速度指令和最大允许归一化转矩传递给 lib_speed_torque_control 函数进行进一步的速度 - 转矩控制。
//void lib_position_speed_torque_control(float position_rad, float max_speed_rad, float max_torque_norm)
//{
//    float speed_rad = position_loop(position_rad);
//    speed_rad = min(fabs(speed_rad), max_speed_rad) * (speed_rad > 0 ? 1 : -1);
//    lib_speed_torque_control(speed_rad, max_torque_norm);
//}
void lib_position_speed_torque_control(float position_rad, float max_speed_rad, float max_torque_norm)
{
    // 调用 position_loop 函数，将目标位置 position_rad 作为输入
    // position_loop 函数通常实现了位置控制算法，例如 PID 控制器
    // 该函数会根据目标位置和当前实际位置的误差，计算出所需的速度指令
    // 计算结果存储在变量 speed_rad 中
    float speed_rad = position_loop(position_rad);

    // 对计算得到的速度指令进行限幅处理
    // fabs(speed_rad) 计算速度指令的绝对值
    // min(fabs(speed_rad), max_speed_rad) 取速度指令绝对值和最大允许速度之间的较小值
    // (speed_rad > 0 ? 1 : -1) 用于保留速度指令的符号（正或负）
    // 最终将限幅后的速度指令重新赋值给 speed_rad
    speed_rad = min(fabs(speed_rad), max_speed_rad) * (speed_rad > 0 ? 1 : -1);

    // 调用 lib_speed_torque_control 函数，将限幅后的速度指令 speed_rad 和最大允许归一化转矩 max_torque_norm 作为参数传入
    // lib_speed_torque_control 函数会根据输入的速度指令和最大允许转矩，进一步执行速度 - 转矩控制操作
    lib_speed_torque_control(speed_rad, max_torque_norm);
}

void set_motor_pid(
    float position_p, float position_i, float position_d,
    float speed_p, float speed_i, float speed_d,
    float torque_d_p, float torque_d_i, float torque_d_d,
    float torque_q_p, float torque_q_i, float torque_q_d)
{
    pid_position.Kp = position_p;
    pid_position.Ki = position_i;
    pid_position.Kd = position_d;

    pid_speed.Kp = speed_p;
    pid_speed.Ki = speed_i;
    pid_speed.Kd = speed_d;

    pid_torque_d.Kp = torque_d_p;
    pid_torque_d.Ki = torque_d_i;
    pid_torque_d.Kd = torque_d_d;

    pid_torque_q.Kp = torque_q_p;
    pid_torque_q.Ki = torque_q_i;
    pid_torque_q.Kd = torque_q_d;
    arm_pid_init_f32(&pid_position, false);
    arm_pid_init_f32(&pid_speed, false);
    arm_pid_init_f32(&pid_torque_d, false);
    arm_pid_init_f32(&pid_torque_q, false);
}
//这个 cycle_diff 函数的作用是处理环路角度差的计算，
//特别是在周期性信号（比如编码器角度或旋转信号）出现跨越零点（即周期性跳变）时，确保返回的角度差值在合理的范围内。：
float cycle_diff(float diff, float cycle)
{
    if (diff > (cycle / 2))
        diff -= cycle;
    else if (diff < (-cycle / 2))
        diff += cycle;
    return diff;
}